This invention relates to tracking a body by nonlinear and non-Gaussian parametric filtering.
Tracking a body, for example a robot or other physical agent, as it moves in a physical space can be based on measurements that depend on the movement of the body in the space. For example, sensor measurements may provide partial or noisy information regarding the position of the body. Over time, such measurements can be accumulated and combined with information related to the anticipated movement of the body to track the motion of the body. In some cases, the tracking is probabilistic in that the knowledge that is gleaned from the measurements is represented as a probability distribution representing the uncertainty in the position. Generally, the process of accumulating the measurements and combining them with the anticipated movement is referred to as a type of filtering. The characteristics of the body, for example, the position, velocity, pose, etc. are together referred to as the “state” of the body.
Filtering can be applied to problems in which a state of a body evolves according to system dynamics and deterministic and/or random inputs to the system, and the system is observered via measurements (“observations”) that depend on the state. A goal of filtering can include recovering an estimate of the evolution of the system state based on measurements. Such filtering of the measurements to recover the system state can be a challenging problem, for example, because the measurements are noisy (i.e., have high degree of uncertainty) or because there may not be a direct way to derive the system state from the measurements even if they were perfectly known. One example where such filtering is useful is in navigation in which the system state to be recovered includes the position and velocity of a vehicle and the measurements include reference based (e.g., GPS or beacon based) and/or interial (e.g., accelerometer or gyroscope based) sensor measurements of the vehicle's position. In the discussion below, the variables y1, y2, . . . refer to the state of the system at ordered discrete times t1, t2, . . . , and the variables z1, z2, . . . refer to the measurements at those times.
One approach to dealing with the uncertainty in the state evolution or the relationship between the system state and sensor measurements is to use probabilistic models, such as a model for the distribution p(yk|yk−1) that characterizes the evolution of the state from time tk−1 to time tk, and a distribution p(zk|yk) that characterizes the measurement distribution at time tk given a particular value yk of the system state at time tk. Further assumptions on the probability distributions can be made, including a Markov assumption on the system evolution p(yk|yk−1, . . . , y1)=p(yk|yk−1) and a conditional independence assumption on the observations such that p(z1, . . . , zk|y1, . . . , yk)=Πip(zi|yi).
The Markov and conditional independence assumptions used in a number of filtering techniques in which the distribution of the state at time tk−1 based on all measurements up to that time, denoted p(yk−1|z1:k−1) where z1:k−1 refers to the sequence z1, . . . , zk−1, is updated based on zk to yield the distribution p(yk|z1:k). In particular, Bayes rule is used as the basis of two steps:    a) Prediction step:Determine p(yk|z1:k−1) from p(yk|yk−1) and p(yk−1|z1:k−1),    b) Update step: Determine p(yk|z1:k) from p(yk|z1:k−1), (zk|yk), and zk.
The prediction step may be implemented in the case of a continuous state space based on the equation    a) Prediction equation: p(yk|z1:k−1)=∫yk−1p(yk|yk−1)p(yk−1|z1:k−1)dyk−1, or in the case of a discrete state space as a corresponding sum over discrete state values yk−1. The update step may be implemented based on the equation    b) Update equation:
            p      ⁡              (                              y            k                    ❘                      z                          1              :              k                                      )              =                  p        ⁡                  (                                    y              k                        ❘                          z                              1                :                                  k                  -                  1                                                              )                    ⁢              p        ⁡                  (                                    z              k                        ❘                          y              k                                )                    ⁢              (                              p            ⁡                          (                              z                                  1                  :                                      k                    -                    1                                                              )                                            p            ⁡                          (                              z                                  1                  :                  k                                            )                                      )              ,which may be broken up into two steps:    b1) Unnormalized update equation: ƒ(yk)=p(yk|z1:k−1)p(zk|yk), and    b2) Normalize:
      p    ⁡          (                        y          k                ❘                  z                      1            :            k                              )        =            f      ⁡              (                  y          k                )                            ∫        y            ⁢                        f          ⁡                      (            y            )                          ⁢                                  ⁢                  ⅆ          y                    
When the initial distribution of the system state is Gaussian as well as both dynamic and measurement models are linear-Gaussian, the prediction and update steps retain the Gaussian nature of the distributions, leading to simple closed-form solutions. The recursion given by such analytic solution is the well-known Kalman Filter. However, linear and Gaussian models are not adequate for navigation systems deployed in harsh environments, causing Kalman filters to suffer severe performance degradations. Other well-know approaches to implementing the prediction and update steps include use of a discrete state space in a Hidden Markov Model.
One approach to dealing with non-linear form of the system or observation model is to use linearize approximations of the models. Generally, some such approaches are referred to as Extended Kalman Filters.
Nonparametric techniques based on Monte Carlo sampling have also been used to handle such complex scenarios but with a computational complexity beyond the reach of many navigation devices, especially those that are small and low-cost. One such approach is refered to as a Particle Filter. Generally, such approaches represent the posterior distribution of the positional state as a set of random samples with associated weights (i.e., a probability density composed of a set of weighted impulses), each representing a “particle.” The prediction and update steps generally involve moving the samples (or possibly removing and adding samples) and determining new weights for the samples.
Another technique called Rao-Blackwellized particle filter takes advantage of the specific structure by marginalizing out some of the variables analytically and solving the remaining variables using particle filters. However, since this method needs to partition the state-space, it is only useful in some specific cases.
The dimension of a filter is the number of real numbers that the filter has to store in each step, where this dimension is a measure of the complexity of the filter. For instance, Kalman-like filters have to store a mean and a covariance matrix of a state vector of size d , therefore their dimension is d(1+(d+1)/2). On the other hand, particle filters have a dimension of M(d+1), where M is the number of particles which usually has to be several orders of magnitude larger than d to achieve acceptable performances.
Each navigation problem has an underlying structure that is determined by the dynamic and measurements models. Generally, traditional techniques for navigation such as the Kalman filter do not capture the nonlinear and non-Gaussian behavior of measurements in wireless localization and navigation systems deployed in harsh environments. Nonparametric filters as particle filters can cope with the measurements behavior in such environments at the expense of a computational complexity beyond the reach of low-cost navigation devices.
In essence, suboptimal filtering techniques choose a family of distributions and perform the prediction, update, and normalization steps in such a way that the estimated posterior always lies within the same family. For example, approaches such as EKF or UKF choose the family of Gaussian distributions and make the approximations by Taylor series expansions or deterministic sample points, respectively. Particle filters use mixtures of deltas as the family to approximate the distribution. The performance of each technique in terms of accuracy and complexity depends on the suitability of the family and accuracy/complexity of the approximation process.